/**
 * @file Predict.h
 * @author enemy1205 (enemy1205@qq.com)
 * @brief 预测类头文件
 * @date 2021-10-10
 */
#pragma once
#include "Compensate.h"

//构造函数均交给子类
class Predict {
public:
    //预测对外api
    virtual void predict(tracker_ptr &tracker, Compensate_ptr &compensate) {};

    //更新序列
    virtual void updateSequence(GyroData &gyroData, clock_t &appear_time) = 0;

    //卡尔曼更新
    virtual void updateKalman(tracker_ptr &tracker, float flying_time) = 0;

    //通用更新PNP数据
    void updatePnP(tracker_ptr &tracker);

protected:
    std::deque<clock_t> time_sequence;//时间戳序列
    deque<GyroData_ptr> _gyroData;       //收到电控发送来的数据,22年赛季可能打符也需陀螺仪数据
    float last_distance;   // 上一帧距离
//    float flying_time;     // 子弹飞行时间,交由补偿实现

    // 计算装甲板的位姿
    virtual ResultPnP calculatePnPData(const Mat &cameraMatrix, const Mat &distcoeff, tracker_ptr &tracker) = 0;

    //二者预测数据维度不同，无法在基类实现
    //void updatePredict(const cv::Matx41f &predict_mat, tracker_ptr &tracker) {};
};

using Predict_ptr = shared_ptr<Predict>;

class ArmorPredict : public Predict {
public:
    ArmorPredict();

    void predict(tracker_ptr &tracker, Compensate_ptr &compensate) override;

    void updateSequence(GyroData &gyroData, clock_t &appear_time) override;

    void updateKalman(tracker_ptr &tracker, float flying_time) override;

protected:
    Point2f target_speed;               // 目标相对速度
    Point2f gyro_speed;                 // 陀螺仪速度
    Point2f Speed;                      // 目标装甲板速度
    ResultPnP calculatePnPData(const Mat &cameraMatrix, const Mat &distcoeff, tracker_ptr &tracker) override;

    //获取陀螺仪速度
    void getGyroSpeed();

    //更新预测信息
    void updatePredict(const cv::Matx41f &predict_mat, tracker_ptr &tracker);

};

class RunePredict : public Predict {
public:
    RunePredict();

    void predict(tracker_ptr &tracker, Compensate_ptr &compensate) override;

    void updateSequence(GyroData &gyroData, clock_t &appear_time) override;

    void updateKalman(tracker_ptr &tracker, float flying_time) override;

    //更新神符信息
    void updateRuneInfo(tracker_ptr &tracker);

protected:
    deque<float> real_angle;    // 真实角度值
    int round;                  // 圈数
    float radius;               // 旋转半径
    float speed;                // 角速度
    float image_predict_angle;  // 图像中的预测角度
    ResultPnP calculatePnPData(const Mat &cameraMatrix, const Mat &distcoeff, tracker_ptr &tracker) override;

    //计算神符旋转真实角度
    float calculateRealAngle(tracker_ptr &tracker);

    //更新预测信息
    void updatePredict(const cv::Matx21f &predict_mat, tracker_ptr &tracker);

    // 获取目标神符旋转中心
    inline cv::Point2f getGoalCenter(tracker_ptr &tracker) { return tracker->getFeatures().front()->getCenter(); }

};